#include "api.h"
#include <string>
#include <filesystem>
#include <unistd.h>
#include <sys/time.h>
#include <csignal>
#include <thread>
#include <sys/stat.h>
#include <iostream>
#include <assert.h>

#include <opencv2/core.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/imgcodecs.hpp>
#include <opencv2/calib3d.hpp>

#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
// #include <pcl_conversions/pcl_conversions.h>

#include "picojson.h"


struct StrcParam{
    // TOF相机参数
    int Width_tof = 0;
    int Height_tof = 0;
    double fx_tof;
    double fy_tof;
    double cx_tof;
    double cy_tof;
    double k1_tof;
    double k2_tof;
    double k3_tof;
    double p1_tof;
    double p2_tof;

    // RGB相机参数
    int Width_rgb = 0;
    int Height_rgb = 0;
    double fx_rgb;
    double fy_rgb;
    double cx_rgb;
    double cy_rgb;
    double k1_rgb;
    double k2_rgb;
    double k3_rgb;
    double p1_rgb;
    double p2_rgb;

    // RGB-D参数
    int Width_rgbd = 0;
    int Height_rgbd = 0;
    int Width_rgbd_cut = 0;
    double fx_rgbd;
    double fy_rgbd;
    double cx_rgbd;
    double cy_rgbd;

    // 外参
    double R[3][3];  // 旋转矩阵
    double T[3];      // 平移向量

    Eigen::Matrix3f R_;
    Eigen::Vector3f T_; 
};
StrcParam mParam;


int parseIntrinsicParameters(const char* pInfo);
void alignDepthToRGB_Resize( 
    cv::Mat &depth_aligned, 
    pcl::PointCloud<pcl::PointXYZ>::Ptr &rgb_cloud, 
    pcl::PointCloud<pcl::PointXYZ>::Ptr &tof_cloud);
int handle_ = -1;

bool rgb_open_;
bool cloud_open_;
std::string ip_;
int exposure_;
int resolution_height_;
bool cut_flag_;
int rgbd_mode_;

int depth_height_;
int depth_width_;
int rgb_height_;
int rgb_width_;

float fx_;
float fy_;
float cx_;
float cy_;
float k1_;
float k2_;
float k3_;
float p1_;
float p2_;

float rgb_fx_;
float rgb_fy_;
float rgb_cx_;
float rgb_cy_;
float rgb_k1_;
float rgb_k2_;
float rgb_p1_;
float rgb_p2_;
float rgb_k3_;

cv::Mat TOF_camera_matrix = cv::Mat::eye(3, 3, CV_32F);
cv::Mat RGB_camera_matrix = cv::Mat::eye(3, 3, CV_32F);

cv::Mat TOF_dist_coeffs = cv::Mat::zeros(1, 5, CV_32F);
cv::Mat RGB_dist_coeffs = cv::Mat::zeros(1, 5, CV_32F);


bool init_flag_;

int param_version_;

